#include "aimage.h"
#include <assert.h>
#include <stdlib.h>
#include <math.h>
#include <stdio.h>

#define RAD_GAIN 10.0f

void ASTAR_FIELD::filter_by_pixels( int num_pixels, bool filter_larger )
{
	int end = 0;
	int i;

	for ( i = 0; i < num_stars; ++i )
	{
		if ( !filter_larger )
		{
			if ( stars[ i ].pixels >= num_pixels )
			{
				stars[ end++ ] = stars[ i ];
			}
		}
		else
		{
			if ( stars[ i ].pixels <= num_pixels )
			{
				stars[ end++ ] = stars[ i ];
			}
		}
	}
	num_stars = end;
}

void ASTAR_FIELD::filter_by_distortion( float ratio )
{
	int end = 0;
	int i;

	for ( i = 0; i < num_stars; ++i )
	{
		if ( stars[ i ].maxrad > .5 && stars[ i ].minrad / stars[ i ].maxrad > ratio )
		{
			stars[ end++ ] = stars[ i ];
		}
	}
	num_stars = end;
}

void ASTAR_FIELD::filter_out_unmapped( void )
{
	int end = 0;
	int i;

	for ( i = 0; i < num_stars; ++i )
	{
		if ( mapping_function[i] != -1 )
		{
			stars[ end++ ] = stars[ i ];
		}
	}
	num_stars = end;

}

float ASTAR_FIELD::get_ratio( void )
{
	int i;
	float ratio = 0;

	for ( i = 0; i < num_stars; ++i )
	{
		float this_ratio = stars[ i ].minrad / stars[ i ].maxrad;
		ratio += this_ratio;
		//fprintf( stderr, "%d %g %g %g\n", i, stars[i].minrad, stars[i].maxrad, this_ratio);
	}
	return ratio / (( float ) num_stars );
}

float ASTAR_FIELD::get_score( void )
{
	int i;
	float avg = 0;

	for ( i = 0; i < num_stars; ++i )
	{
		float this_score = stars[ i ].maxrad;
		avg += this_score;
	}
	return avg / (( float ) num_stars );
}

void ASTAR_FIELD::set_star( int num, const ASTAR_DATA &star )
{
	if ( num >= num_stars )
	{
		assert( num == num_stars );
		num_stars++;
		assert( num_stars < max_stars );
	}

	stars[ num ] = star;
}

const ASTAR_DATA &ASTAR_FIELD::get_star( int num ) const
{
	assert( num >= 0 && num < num_stars );
	return stars[ num ];
}


int sort_compare_function( const void *ve0, const void *ve1 )
{
	ASTAR_GAIN *e0 = ( ASTAR_GAIN * ) ve0;
	ASTAR_GAIN *e1 = ( ASTAR_GAIN * ) ve1;

	if ( e0->dist > e1->dist )
	{
		return 1;
	}
	if ( e0->dist < e1->dist )
	{
		return -1;
	}
	return 0;
}

void ASTAR_FIELD::map_to( const ASTAR_FIELD &other_field )
{
	int i;
	int index;
	int gain_list_size = num_stars * other_field.num_stars;
	bool src_allocated[ max_stars ];
	bool dst_allocated[ max_stars ];

	for ( i= 0; i < num_stars; ++i )
	{
		src_allocated[ i ] = false;
		mapping_function[ i ] = -1;
	}
	for ( i= 0; i < other_field.num_stars; ++i )
	{
		dst_allocated[ i ] = false;
	}

	ASTAR_GAIN *gain_list = new ASTAR_GAIN[ gain_list_size ];

	for ( i = 0, index=0 ; i < num_stars; ++i )
	{
		int j;
		for ( j = 0; j < other_field.num_stars; ++j )
		{
			float xd, yd, minrd, maxrd;
			gain_list[ index ].src = i;
			gain_list[ index ].dst = j;

			xd = other_field.stars[ j ].xc - stars[ i ].xc;
			yd = other_field.stars[ j ].yc - stars[ i ].yc;
			minrd = other_field.stars[ j ].minrad - stars[ i ].minrad;
			maxrd = other_field.stars[ j ].maxrad - stars[ i ].maxrad;

			gain_list[ index ].dist = sqrtf( xd * xd + yd * yd ) + fabs( maxrd * RAD_GAIN ) + fabs( minrd * RAD_GAIN );
			index++;
		}
	}
	qsort( gain_list, gain_list_size,
			sizeof( ASTAR_GAIN ), sort_compare_function );

	for ( index = 0; index < gain_list_size; ++index )
	{
		ASTAR_GAIN *gain = gain_list + index;

		if ( !src_allocated[ gain->src ] && !dst_allocated[ gain->dst ] )
		{
			mapping_function[ gain->src ] = gain->dst;
			src_allocated[ gain->src ] = true;
			dst_allocated[ gain->dst ] = true;
		} 
	}
	delete[] gain_list;
}

int pixel_sort_function( const void *ve0, const void *ve1 )
{
	ASTAR_DATA *e0 = ( ASTAR_DATA * ) ve0;
	ASTAR_DATA *e1 = ( ASTAR_DATA * ) ve1;

	return e1->pixels - e0->pixels;
}

int goodness_sort_function( const void *ve0, const void *ve1 )
{
	ASTAR_DATA *e0 = ( ASTAR_DATA * ) ve0;
	ASTAR_DATA *e1 = ( ASTAR_DATA * ) ve1;

	if ( e1->goodness - e0->goodness < 0 )
	{
		return -1;
	}
	if ( e1->goodness - e0->goodness > 0 )
	{
		return 1;
	}
	return 0;
}

void ASTAR_FIELD::top_x_by_pixels( int x )
{
	assert( x <= num_stars );
	qsort( stars, num_stars, sizeof( ASTAR_DATA), pixel_sort_function );
	num_stars = x;
}

void ASTAR_FIELD::top_x_by_goodness( int x )
{
	assert( x <= num_stars );
	qsort( stars, num_stars, sizeof( ASTAR_DATA), goodness_sort_function );
	num_stars = x;
}

void ASTAR_FIELD::recompute_map( const ASTAR_FIELD &other_field, const ASTAR_TRANSFORM &transform )
{
	ASTAR_FIELD tmp_field;
	tmp_field.transform( other_field, transform );
	map_to( tmp_field );
}

float ASTAR_FIELD::avg_distance( const ASTAR_FIELD &other_field, const ASTAR_TRANSFORM &transform  )
{
	ASTAR_FIELD tmp_field;
	int i;
	float total_d = 0.0f;
	float samples = 0.0f;

	tmp_field.transform( other_field, transform );

	for ( i = 0; i < num_stars; ++i )
	{
		float weight;
		int mapto = mapping_function[ i ];

		if ( mapto != -1 )
		{ 
			float xs, ys, xd, yd, xdiff, ydiff, minrd, maxrd;

			xs = stars[ i ].xc;
			ys = stars[ i ].yc;
			xd = tmp_field.stars[ mapto ].xc;
			yd = tmp_field.stars[ mapto ].yc;

			xdiff = xs - xd;
			ydiff = ys - yd;
			minrd = stars[ i ].minrad - tmp_field.stars[ mapto ].minrad;
			maxrd = stars[ i ].maxrad / tmp_field.stars[ mapto ].maxrad;

			//fprintf(stderr,"   %d->%d %g %g (%g)\n", i, mapto, xdiff, ydiff, 
			//	sqrtf( xdiff * xdiff + ydiff * ydiff ));


			weight = maxrd;
			if ( weight > 0 )
			{
				weight = 1.0f / weight;
			}
			//weight = weight * weight;
			weight = 1.0;

			total_d += sqrtf( xdiff * xdiff + ydiff * ydiff );
			samples += weight;
		}
	}
	if ( samples == 0.0f )
	{
		return 0.0f;
	}
	return total_d / samples;
}

void ASTAR_FIELD::transform( const ASTAR_FIELD &other_field, const ASTAR_TRANSFORM &transform )
{
	int i;

	const float xc = 2601 / 2;
	const float yc = 1732 / 2;

	for ( i = 0; i < other_field.num_stars; ++i )
	{
		float x, y, xt, yt;

		x = other_field.stars[ i ].xc - xc;
		y = other_field.stars[ i ].yc - yc;

		xt = cosf( transform.angle ) * x - sinf( transform.angle ) * y;
		yt = sinf( transform.angle ) * x + cosf( transform.angle ) * y;

		xt += xc + transform.xo;
		yt += yc + transform.yo;
			
		stars[ i ].xc = xt;
		stars[ i ].yc = yt;
		stars[ i ].pixels = other_field.stars[ i ].pixels;
		stars[ i ].minrad= other_field.stars[ i ].minrad;
		stars[ i ].maxrad= other_field.stars[ i ].maxrad;
	}
	num_stars = other_field.num_stars;
}

ASTAR_TRANSFORM ASTAR_FIELD::synch_fields( const ASTAR_FIELD &other_field, ASTAR_TRANSFORM t )
{
	//ASTAR_TRANSFORM t;
	int temp;
	float mult = 4.0f;

	recompute_map( other_field, t );
	float score = avg_distance( other_field, t );

	for ( temp = 0; temp < 12; ++temp )
	{
		int i;
		for ( i = 0; i < 100; ++i )
		{
			ASTAR_TRANSFORM prop_t;

			prop_t = t;

			switch ( rand() % 3 )
			{
				case 0:
					prop_t.xo += ((float) ( rand() % 5 ) - 3) * mult;
					break;
				case 1:
					prop_t.yo += ((float) ( rand() % 5 ) - 3) * mult;
					break;
				case 2:
					prop_t.angle += ((float) ( rand() % 5 ) - 3) / 3200.0f * mult;
					break;
			}

			float newscore = avg_distance( other_field, prop_t );

			if ( newscore < score )
			{
				t = prop_t;
				recompute_map( other_field, t );
				score = avg_distance( other_field, t );
				//fprintf( stderr, "%d %d score %f xo %f yo %f angle %f\n",
				//	temp, i, newscore, prop_t.xo, prop_t.yo, prop_t.angle );
 			}
		}
		mult = mult * .7f;
	}
	return t;
}

void ASTAR_FIELD::compute_goodness( const AIMAGE &congestion_map, float cong_weight  )
{
	int i;

	for ( i = 0; i < num_stars; ++i )
	{
		float ratio;
		float pixels;
		float congestion;
		int xc, yc;

		if ( stars[ i ].maxrad != 0.0f )
		{
			ratio = stars[ i ].minrad / stars[ i ].maxrad;
			ratio = ratio * ratio;
		}
		else
		{
			ratio = 0.0f;
		}

		pixels = stars[ i ].pixels;
		
		xc = stars[ i ].xc;
		yc = stars[ i ].yc;

		xc /= congestion_scale;
		yc /= congestion_scale;

		APIXEL p;
		congestion_map.getpixel( xc, yc, p );

		congestion = p.r;
		congestion = 1.0f - congestion / (( float ) FULL_INT );	
		congestion = congestion * congestion;
		congestion = congestion * cong_weight;

		stars[ i ].goodness = congestion * pixels * ratio;
	}
}

void ASTAR_FIELD::reverse_map( const ASTAR_FIELD &field )
{
	int i;

	for ( i = 0; i < num_stars; ++i )
	{
		mapping_function[ i ] = -1;
	}

	for ( i = 0; i < field.num_stars; ++i )
	{
		int map = field.mapping_function[ i ];
		assert( map >= 0 && map < num_stars );
		mapping_function[ map ] = i;
	}
}


